/*
 * Copyright (c) 2003-2009 jMonkeyEngine
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the distribution.
 *
 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 
 *   may be used to endorse or promote products derived from this software 
 *   without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

package unwrap;

import toxi.geom.Vec2D;
import toxi.geom.Vec3D;

/**
 * <code>Intersection</code> provides functional methods for calculating the
 * intersection of some objects. All the methods are static to allow for quick
 * and easy calls. <code>Intersection</code> relays requests to specific classes
 * to handle the actual work. By providing checks to just
 * <code>BoundingVolume</code> the client application need not worry about what
 * type of bounding volume is being used.
 * 
 * @author Mark Powell
 * @version $Id: Intersection.java 4131 2009-03-19 20:15:28Z blaine.dev $
 */
class UIntersection {

	/**
	 * EPSILON represents the error buffer used to denote a hit.
	 */
	public static final double EPSILON = 1e-12;

	private static final Vec3D tempVa = new Vec3D();

	private static final Vec3D tempVb = new Vec3D();

	private static final Vec3D tempVc = new Vec3D();

	private static final Vec3D tempVd = new Vec3D();

	private static final Vec3D tempVe = new Vec3D();

	private static final float[] tempFa = new float[2];

	private static final float[] tempFb = new float[2];

	private static final Vec2D tempV2a = new Vec2D();

	private static final Vec2D tempV2b = new Vec2D();


	/**
	 * This method tests for the intersection between two triangles defined by
	 * their vertexes. Converted to java from C code found at
	 * http://www.acm.org/jgt/papers/Moller97/tritri.html
	 */	

	public static boolean intersection2d(UTriangle t1, UTriangle t2) {
		
		Vec2D v0 = new Vec2D(t1.getA2d().x, t1.getA2d().y).interpolateTo(t1.getIncenter2d(), 0.1f);
		Vec2D v1 = new Vec2D(t1.getB2d().x, t1.getB2d().y).interpolateTo(t1.getIncenter2d(), 0.1f);
		Vec2D v2 = new Vec2D(t1.getC2d().x, t1.getC2d().y).interpolateTo(t1.getIncenter2d(), 0.1f);

		Vec2D u0 = new Vec2D(t2.getA2d().x, t2.getA2d().y).interpolateTo(t2.getIncenter2d(), 0.1f);
		Vec2D u1 = new Vec2D(t2.getB2d().x, t2.getB2d().y).interpolateTo(t2.getIncenter2d(), 0.1f);
		Vec2D u2 = new Vec2D(t2.getC2d().x, t2.getC2d().y).interpolateTo(t2.getIncenter2d(), 0.1f);
		
		return intersection(toVec3D(v0),toVec3D(v1),toVec3D(v2),toVec3D(u0),toVec3D(u1),toVec3D(u2));
	}
	
	private static Vec3D toVec3D( Vec2D v2d){
		return new Vec3D(v2d.x, v2d.y, 0);
	}
	
	public static boolean intersection(UTriangle t1, UTriangle t2) {
		
		Vec3D v0 = new Vec3D(t1.a.x, t1.a.y, t1.a.z).interpolateTo(t1.getIncenter3d(), 0.1f);
		Vec3D v1 = new Vec3D(t1.b.x, t1.b.y, t1.b.z).interpolateTo(t1.getIncenter3d(), 0.1f);
		Vec3D v2 = new Vec3D(t1.c.x, t1.c.y, t1.c.z).interpolateTo(t1.getIncenter3d(), 0.1f);

		Vec3D u0 = new Vec3D(t2.a.x, t2.a.y, t2.a.z).interpolateTo(t2.getIncenter3d(), 0.1f);
		Vec3D u1 = new Vec3D(t2.b.x, t2.b.y, t2.b.z).interpolateTo(t2.getIncenter3d(), 0.1f);
		Vec3D u2 = new Vec3D(t2.c.x, t2.c.y, t2.c.z).interpolateTo(t2.getIncenter3d(), 0.1f);
		
		return intersection(v0,v1,v2,u0,u1,u2);
	}
	
	public static boolean intersection2d(Vec2D v0, Vec2D v1, Vec2D v2, Vec2D u0, Vec2D u1, Vec2D u2){
		return intersection(toVec3D(v0),toVec3D(v1),toVec3D(v2),toVec3D(u0),toVec3D(u1),toVec3D(u2));
	}
	
	
	public static boolean intersection(Vec3D v0, Vec3D v1, Vec3D v2, Vec3D u0, Vec3D u1, Vec3D u2) {
		
		Vec3D e1 = tempVa;
		Vec3D e2 = tempVb;
		Vec3D n1 = tempVc;
		Vec3D n2 = tempVd;
		float d1, d2;
		float du0, du1, du2, dv0, dv1, dv2;
		Vec3D d = tempVe;
		float[] isect1 = tempFa;
		float[] isect2 = tempFb;
		float du0du1, du0du2, dv0dv1, dv0dv2;
		short index;
		float vp0, vp1, vp2;
		float up0, up1, up2;
		float bb, cc, max;
		float xx, yy, xxyy, tmp;

		/* compute plane equation of triangle(v0,v1,v2) */
		e1 = v1.sub(v0);
		e2 = v2.sub(v0);
		n1 = e1.cross(e2);

		d1 = -n1.dot(v0);
		/* plane equation 1: n1.X+d1=0 */

		/*
		 * put u0,u1,u2 into plane equation 1 to compute signed distances to the
		 * plane
		 */
		du0 = n1.dot(u0) + d1;
		du1 = n1.dot(u1) + d1;
		du2 = n1.dot(u2) + d1;

		/* coplanarity robustness check */
		if (Math.abs(du0) < EPSILON)
			du0 = 0.0f;
		if (Math.abs(du1) < EPSILON)
			du1 = 0.0f;
		if (Math.abs(du2) < EPSILON)
			du2 = 0.0f;
		du0du1 = du0 * du1;
		du0du2 = du0 * du2;

		if (du0du1 > 0.0f && du0du2 > 0.0f) {
			return false;
		}

		/* compute plane of triangle (u0,u1,u2) */
		e1 = u1.sub(u0);
		e2 = u2.sub(u0);
		n2 = e1.cross(e2);

		d2 = -n2.dot(u0);
		/* plane equation 2: n2.X+d2=0 */

		/* put v0,v1,v2 into plane equation 2 */
		dv0 = n2.dot(v0) + d2;
		dv1 = n2.dot(v1) + d2;
		dv2 = n2.dot(v2) + d2;

		if (Math.abs(dv0) < EPSILON)
			dv0 = 0.0f;
		if (Math.abs(dv1) < EPSILON)
			dv1 = 0.0f;
		if (Math.abs(dv2) < EPSILON)
			dv2 = 0.0f;

		dv0dv1 = dv0 * dv1;
		dv0dv2 = dv0 * dv2;

		if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) { /*
											 * same sign on all of them + not
											 * equal 0 ?
											 */
			return false; /* no intersection occurs */
		}

		/* compute direction of intersection line */
		d = n1.cross(n2);

		/* compute and index to the largest component of d */
		max = Math.abs(d.x);
		index = 0;
		bb = Math.abs(d.y);
		cc = Math.abs(d.z);
		if (bb > max) {
			max = bb;
			index = 1;
		}
		if (cc > max) {
			max = cc;
			vp0 = v0.z;
			vp1 = v1.z;
			vp2 = v2.z;

			up0 = u0.z;
			up1 = u1.z;
			up2 = u2.z;

		} else if (index == 1) {
			vp0 = v0.y;
			vp1 = v1.y;
			vp2 = v2.y;

			up0 = u0.y;
			up1 = u1.y;
			up2 = u2.y;
		} else {
			vp0 = v0.x;
			vp1 = v1.x;
			vp2 = v2.x;

			up0 = u0.x;
			up1 = u1.x;
			up2 = u2.x;
		}

		/* compute interval for triangle 1 */
		Vec3D abc = tempVa;
		Vec2D x0x1 = tempV2a;
		if (newComputeIntervals(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, abc, x0x1)) {
			return coplanarTriTri(n1, v0, v1, v2, u0, u1, u2);
		}

		/* compute interval for triangle 2 */
		Vec3D def = tempVb;
		Vec2D y0y1 = tempV2b;
		if (newComputeIntervals(up0, up1, up2, du0, du1, du2, du0du1, du0du2, def, y0y1)) {
			return coplanarTriTri(n1, v0, v1, v2, u0, u1, u2);
		}

		xx = x0x1.x * x0x1.y;
		yy = y0y1.x * y0y1.y;
		xxyy = xx * yy;

		tmp = abc.x * xxyy;
		isect1[0] = tmp + abc.y * x0x1.y * yy;
		isect1[1] = tmp + abc.z * x0x1.x * yy;

		tmp = def.x * xxyy;
		isect2[0] = tmp + def.y * xx * y0y1.y;
		isect2[1] = tmp + def.z * xx * y0y1.x;

		sort(isect1);
		sort(isect2);

		if (isect1[1] < isect2[0] || isect2[1] < isect1[0]) {
			return false;
		}

		return true;
	}

	private static void sort(float[] f) {
		if (f[0] > f[1]) {
			float c = f[0];
			f[0] = f[1];
			f[1] = c;
		}
	}

	private static boolean newComputeIntervals(float vv0, float vv1, float vv2, float d0, float d1, float d2, float d0d1, float d0d2, Vec3D abc, Vec2D x0x1) {
		if (d0d1 > 0.0f) {
			/* here we know that d0d2 <=0.0 */
			/*
			 * that is d0, d1 are on the same side, d2 on the other or on the
			 * plane
			 */
			abc.x = vv2;
			abc.y = (vv0 - vv2) * d2;
			abc.z = (vv1 - vv2) * d2;
			x0x1.x = d2 - d0;
			x0x1.y = d2 - d1;
		} else if (d0d2 > 0.0f) {
			/* here we know that d0d1 <=0.0 */
			abc.x = vv1;
			abc.y = (vv0 - vv1) * d1;
			abc.z = (vv2 - vv1) * d1;
			x0x1.x = d1 - d0;
			x0x1.y = d1 - d2;
		} else if (d1 * d2 > 0.0f || d0 != 0.0f) {
			/* here we know that d0d1 <=0.0 or that d0!=0.0 */
			abc.x = vv0;
			abc.y = (vv1 - vv0) * d0;
			abc.z = (vv2 - vv0) * d0;
			x0x1.x = d0 - d1;
			x0x1.y = d0 - d2;
		} else if (d1 != 0.0f) {
			abc.x = vv1;
			abc.y = (vv0 - vv1) * d1;
			abc.z = (vv2 - vv1) * d1;
			x0x1.x = d1 - d0;
			x0x1.y = d1 - d2;
		} else if (d2 != 0.0f) {
			abc.x = vv2;
			abc.y = (vv0 - vv2) * d2;
			abc.z = (vv1 - vv2) * d2;
			x0x1.x = d2 - d0;
			x0x1.y = d2 - d1;
		} else {
			/* triangles are coplanar */
			return true;
		}
		return false;
	}

	private static boolean coplanarTriTri(Vec3D n, Vec3D v0, Vec3D v1, Vec3D v2, Vec3D u0, Vec3D u1, Vec3D u2) {
		Vec3D a = new Vec3D();
		short i0, i1;
		a.x = Math.abs(n.x);
		a.y = Math.abs(n.y);
		a.z = Math.abs(n.z);

		if (a.x > a.y) {
			if (a.x > a.z) {
				i0 = 1; /* a[0] is greatest */
				i1 = 2;
			} else {
				i0 = 0; /* a[2] is greatest */
				i1 = 1;
			}
		} else /* a[0] <=a[1] */{
			if (a.z > a.y) {
				i0 = 0; /* a[2] is greatest */
				i1 = 1;
			} else {
				i0 = 0; /* a[1] is greatest */
				i1 = 2;
			}
		}

		/* test all edges of triangle 1 against the edges of triangle 2 */
		float[] v0f = new float[3];
		v0f = v0.toArray();
		float[] v1f = new float[3];
		v1f = v1.toArray();
		float[] v2f = new float[3];
		v2f = v2.toArray();
		float[] u0f = new float[3];
		u0f = u0.toArray();
		float[] u1f = new float[3];
		u1f = u1.toArray();
		float[] u2f = new float[3];
		u2f = u2.toArray();
		if (edgeAgainstTriEdges(v0f, v1f, u0f, u1f, u2f, i0, i1)) {
			return true;
		}

		if (edgeAgainstTriEdges(v1f, v2f, u0f, u1f, u2f, i0, i1)) {
			return true;
		}

		if (edgeAgainstTriEdges(v2f, v0f, u0f, u1f, u2f, i0, i1)) {
			return true;
		}

		/* finally, test if tri1 is totally contained in tri2 or vice versa */
		pointInTri(v0f, u0f, u1f, u2f, i0, i1);
		pointInTri(u0f, v0f, v1f, v2f, i0, i1);

		return false;
	}

	private static boolean pointInTri(float[] V0, float[] U0, float[] U1, float[] U2, int i0, int i1) {
		float a, b, c, d0, d1, d2;
		/* is T1 completly inside T2? */
		/* check if V0 is inside tri(U0,U1,U2) */
		a = U1[i1] - U0[i1];
		b = -(U1[i0] - U0[i0]);
		c = -a * U0[i0] - b * U0[i1];
		d0 = a * V0[i0] + b * V0[i1] + c;

		a = U2[i1] - U1[i1];
		b = -(U2[i0] - U1[i0]);
		c = -a * U1[i0] - b * U1[i1];
		d1 = a * V0[i0] + b * V0[i1] + c;

		a = U0[i1] - U2[i1];
		b = -(U0[i0] - U2[i0]);
		c = -a * U2[i0] - b * U2[i1];
		d2 = a * V0[i0] + b * V0[i1] + c;
		if (d0 * d1 > 0.0 && d0 * d2 > 0.0)
			return true;

		return false;
	}

	private static boolean edgeAgainstTriEdges(float[] v0, float[] v1, float[] u0, float[] u1, float[] u2, int i0, int i1) {
		float aX, aY;
		aX = v1[i0] - v0[i0];
		aY = v1[i1] - v0[i1];
		/* test edge u0,u1 against v0,v1 */
		if (edgeEdgeTest(v0, u0, u1, i0, i1, aX, aY)) {
			return true;
		}
		/* test edge u1,u2 against v0,v1 */
		if (edgeEdgeTest(v0, u1, u2, i0, i1, aX, aY)) {
			return true;
		}
		/* test edge u2,u1 against v0,v1 */
		if (edgeEdgeTest(v0, u2, u0, i0, i1, aX, aY)) {
			return true;
		}
		return false;
	}

	private static boolean edgeEdgeTest(float[] v0, float[] u0, float[] u1, int i0, int i1, float aX, float Ay) {
		float Bx = u0[i0] - u1[i0];
		float By = u0[i1] - u1[i1];
		float Cx = v0[i0] - u0[i0];
		float Cy = v0[i1] - u0[i1];
		float f = Ay * Bx - aX * By;
		float d = By * Cx - Bx * Cy;
		if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f)) {
			float e = aX * Cy - Ay * Cx;
			if (f > 0) {
				if (e >= 0 && e <= f)
					return true;
			} else {
				if (e <= 0 && e >= f)
					return true;
			}
		}
		return false;
	}
}